/*  --------------------------- (C) COPYRIGHT 2020 Fortiortech ShenZhen -----------------------------
    File Name      : MotorControl.c
    Author         : Ray
    Version        : V0.1
    Date           : 2021-01-18
    Description    : 状态机以及状态机函数.
    ----------------------------------------------------------------------------------------------------
                                       All Rights Reserved
    ------------------------------------------------------------------------------------------------- */
#include <MyProject.h>
/* Private variables ----------------------------------------------------------------------------*/
MotStaType mcState;
MotStaM McStaSet;
uint16 tese;
/*  -------------------------------------------------------------------------------------------------
    Function Name  : MC_Control
    Description    : 电机控制状态机
    Date           : 2021-01-18
    Parameter      : None
    ------------------------------------------------------------------------------------------------- */
void MC_Control(void)
{
    switch (mcState)
    {
        case mcReady:
        {
            Motor_Ready();
            
            if ((MotorSpeed.FlagONOFF == 1) && (mcCurOffset.OffsetFlag == 1))
            {
                mcFocCtrl.State_Count = 0;
                mcState = mcInit;

            }
        }
        break;
        
        case mcInit:
        {
            if (mcFocCtrl.State_Count == 0) //为了使得初始化只进行一次，思考使用联合体 McStaSet.SetFlag.ChargeSetFlag 设置标志位
            {
                Motor_Init(); //电机相关参数初始化
                MOE = 0;      //关闭输出
                
                if (FRMODE ? (FR == OPEN) : (FR == CLOSE))  //强拖阶段还要再次设置正反转设置
                {
                    ClrBit(DRV_CR, DDIR); // 正转/* code */
                }
                else
                {
                    SetBit(DRV_CR, DDIR); // 反转
                }
                
                mcState = mcCharge; // mcTailWind;
            }
        }
        break;
        
        
        case mcCharge:
        {
            if (mcFocCtrl.State_Count == 0)
            {
                #if (IPMState == NormalRun) // 正常运行模式
                MOE = 0;
                BL_S_ParaInit(); // BLDC启动算法参数初始化
                mcState = mcStart;
                #elif (IPMState == IPMtest) // 桥驱验证
                Motor_Charge(); // 6832系列芯片因为使用3P3N的架构，所以不用进行预充电，用于三相输出检测
                #endif
            }
        }
        break;
        
    
        case mcStart:
        {
            Motor_Start(); //电机启动函数相关变量初始化
            
            if (StartProtectEnable == 1) //启动保护使能
            {
                Fault_Start();
            }
            
            if (MotorSpeed.FlagONOFF == 0)
            {
                MOE = 0;
                ONOFFTest.StopFlag = 0;
                mcState = mcStop;
            }
        }
        break;
        
        case mcRun:
        {
            
            if (MotorSpeed.FlagONOFF == 0)
            {
                ONOFFTest.StopFlag = 0;
                MOE = 0;
                mcState = mcStop;
            }
            
            if ((StallProtectEnable == 1) && (mcFaultDect.Stall_flag == 1))
            {
                mcFaultSource = FaultStall;
            }
        }
        break;
        
        case mcStop:
        {
            if (ONOFFTest.StopFlag == 0)
            {
                //软件变量
                BLDC.BLDCSetFlag = 0;
                ONOFFTest.PowerOFF_Count = StopWaitTime;
                ONOFFTest.StopFlag = 1;
                //关输出，清寄存器
                MOE = 0;
                TIM1_CR0 = 0;
                TIM1_CR1 = 0;
                TIM1_CR2 = 0;
                TIM1_CR3 = 0;
                TIM1_CR4 = 7;
                TIM1_IER = 0;
                FOC_CR1 = 0x00;
                DRV_DR = 0;
                ClrBit(DRV_CR, FOCEN);
                mcState = mcBrake;
            }
            
            if ((mcFaultSource == FaultNoSource) ||                               //电机没有发生错误，则可以进行状态切换
                ((mcFaultSource != FaultNoSource) && (MotorSpeed.FlagONOFF == 0)) //电机发生错误，关闭输出后可以进行状态切换
            )
            {
                mcState = mcBrake;
            }
        }
        break;
        
        case mcBrake:
        {          
            if (MotorSpeed.FlagONOFF == 1) 
            {
                mcState = mcReady;

            }
        }
        break;
        
        case mcFault:
        {
            MOE = 0;
            ONOFFTest.StopFlag = 0;
            mcState = mcStop;
					 
        }
        break;
        
        default:
            break;
    }
}
